#include "ros_usbcapture/RosVideoReader.h"

#define VIDEO_PATH 0

namespace wmj
{
    RosVideoReader::RosVideoReader(const rclcpp::NodeOptions & options) : Node("RosVideoReader", options)
    {
        cap.open(VIDEO_PATH);
        imagePublisher = this->create_publisher<base_interfaces::msg::MatWithTime>("camera_image", 10);
        publishImageTimer = this->create_wall_timer(std::chrono::milliseconds(30), std::bind(&RosVideoReader::publishImage, this));
    }

    RosVideoReader::~RosVideoReader(){}

    void RosVideoReader::publishImage()
    {
        // 利用成员变量 capture 读图
        wmj::MatWithTime frame;
        cap >> frame.m_img;
        if (frame.m_img.empty())
        {
            cv::Mat empty(1024, 1280, CV_8UC3, cv::Scalar::all(0));
            frame.m_img = empty;
            cv::putText(frame.m_img, "No Image", cv::Point(400, 400), cv::FONT_HERSHEY_COMPLEX, 3, cv::Scalar::all(255), 1);
            cv::putText(frame.m_img, "This frame", cv::Point(350, 500), cv::FONT_HERSHEY_COMPLEX, 3, cv::Scalar::all(255), 1);
            cv::putText(frame.m_img, "is empty!", cv::Point(350, 600), cv::FONT_HERSHEY_COMPLEX, 3, cv::Scalar::all(255), 1);
        }

        // 创建发布消息
        auto msg = std::make_unique<base_interfaces::msg::MatWithTime>();

        // 类型转换，将 cv::Mat 转换为 sensor_msgs 下的 image
        auto img_msg = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", frame.m_img);
        msg->image = *img_msg.toImageMsg();

        // 添加消息内容
        msg->orientation = frame.m_orientation;
        msg->time_stamp = frame.m_time_stamp;
        msg->trigger = false;

        // 智能指针，左值发布
        // imagePublisher->publish(std::move(msg));

        // 测帧率使用
        //  double  t = wmj::now();
        //  msg->true_time_stamp = t;

        imagePublisher->publish(std::move(msg));

        RCLCPP_INFO(this->get_logger(), "camera_msg has been sent!!");
    }
}

#include "rclcpp_components/register_node_macro.hpp"

RCLCPP_COMPONENTS_REGISTER_NODE(wmj::RosVideoReader)